#include "../Test/Test.h"
#include "../../src/Navigation/Gps.h"
#include "../../src/Communication/TestStream.h"

#include <iostream>

namespace uav
{
namespace test
{

using namespace comm;
using namespace nav;
using namespace math;

class GpsTest : public Suite
{
public:
	GpsTest()
	{
		TEST_ADD(GpsTest::gpgga);
		TEST_ADD(GpsTest::pgrmv);
		TEST_ADD(GpsTest::mixed);
	}

protected:
	virtual void setup()
	{
	}

	virtual void tear_down()
	{
	}

private:

	void gpgga()
	{
		TestStream stream("");
		Gps gps(stream);

		stream.addInput("qwlekrjf\r\n$GPGGA,124415,3324.2528,N,11156.3455,W,2,7,1.0,301.6,M,299.4,M,,*AB\r\n");

		gps.nextSentence();
		TEST_ASSERT_EQUAL(Gps::FIX_QUALITY_DIFFERENTIAL, gps.getFixQuality());
		TEST_ASSERT_EQUAL(Gps::SENTENCE_GPGGA, gps.getSentenceType());

		GeoCoord geo = gps.getPosition();
		TEST_ASSERT_DELTA(0.5830135183, geo.latitude.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-1.953705700, geo.longitude.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(301.6, geo.altitude.toDouble(), 0.000001);
	}

	void pgrmv()
	{
		TestStream stream("");
		Gps gps(stream);

		stream.addInput("$GPGGA,124415,3324.2528,N,11156.3455,W,2,7,1.0,301.6,M,299.4,M,,*AB\r\n");
		stream.addInput("$PGRMV,5.2,-0.3,0.5*AA\r\n$PGRMV,4.9,1.2,-0.1*BB\r\n");

		gps.nextSentence();
		TEST_ASSERT_EQUAL(Gps::FIX_QUALITY_DIFFERENTIAL, gps.getFixQuality());
		TEST_ASSERT_EQUAL(Gps::SENTENCE_GPGGA, gps.getSentenceType());

		gps.nextSentence();
		TEST_ASSERT_EQUAL(Gps::FIX_QUALITY_DIFFERENTIAL, gps.getFixQuality());
		TEST_ASSERT_EQUAL(Gps::SENTENCE_PGRMV, gps.getSentenceType());
		Vector3 v = gps.getVelocity();
		TEST_ASSERT_DELTA(5.2, v.x.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-0.3, v.y.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(0.5, v.z.toDouble(), 0.000001);

		gps.nextSentence();
		TEST_ASSERT_EQUAL(Gps::FIX_QUALITY_DIFFERENTIAL, gps.getFixQuality());
		TEST_ASSERT_EQUAL(Gps::SENTENCE_PGRMV, gps.getSentenceType());
		v = gps.getVelocity();
		TEST_ASSERT_DELTA(4.9, v.x.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(1.2, v.y.toDouble(), 0.000001);
		TEST_ASSERT_DELTA(-0.1, v.z.toDouble(), 0.000001);
	}

	void mixed()
	{

	}
};

}} // end namespace

using namespace uav::test;

int main()
{
	TextOutput output(TextOutput::Verbose);
	GpsTest suite;
	return suite.run(output);
}
